
import time
import wx
import vtk
import math
import numpy
import sys, os
import BaseCalc
import superthread

try:
    import extended.PatternViewer as PatternViewer
except ImportError:
    import PatternViewer

def create(parent, root):    
    return AngleCalculations(parent, root)
    
class AngleCalculations:
        
    def __init__(self, parent, root):
        print "AngleCalculations module loaded."
        if parent == None:
            self.parent = self
            self._root = self
        else:
            self.parent = parent
            self._root = root
        self.isMain = False

        self.appdir = self.ReadRootDirectory()
        self.patternViewerCreated = False
        self.blPos = []
        self.stanceCollectionBinnedRight = []
        self.stanceCollectionBinnedLeft = []




    def CreatePatternViewer(self):
        if not self.patternViewerCreated:
            self.patternViewer = PatternViewer.create(self, self._root, self.appdir)
            self.patternViewerCreated = True            
        
    def Angles(self):
        #presumes that self.blPos has been set up correctly
        
        self.GlobalOXYZ = [[0,0,0], [1,0,0], [0,1,0], [0,0,1]]
        Thoraxstance = self.GenericAngleCalculation(self.GlobalOXYZ, self.OXYZThorax)      #elevatie, elevatievlak, axial
        Claviclestance = self.GenericAngleCalculation(self.OXYZThorax, self.Right_OXYZClavicle)
        Scapulastance = self.GenericAngleCalculation(self.Right_OXYZClavicle, self.Right_OXYZScapula)
        Humerusstance = self.GenericAngleCalculation(self.Right_OXYZScapula, self.Right_OXYZHumerus)
        Forearmstance = self.GenericAngleCalculation(self.Right_OXYZHumerus, self.Right_OXYZForearm)
        #~ Forearmstance[1] = Forearmstance[1] - 90 # elevatievlak
        HumerusThorax = self.GenericAngleCalculation(self.OXYZThorax, self.Right_OXYZHumerus)
        stance_total_right = []
        
        #relative to parent
        stance_total_right[len(stance_total_right):] = Thoraxstance
        stance_total_right[len(stance_total_right):] = Claviclestance
        stance_total_right[len(stance_total_right):] = Scapulastance
        stance_total_right[len(stance_total_right):] = Humerusstance
        stance_total_right[len(stance_total_right):] = Forearmstance
        
        #relative to thorax
        stance_total_right[len(stance_total_right):] = HumerusThorax

        stance_binned_right = []
        stance_binned_right.append(Scapulastance[0]/10)
        stance_binned_right.append(Scapulastance[1]/10)
        stance_binned_right.append(Scapulastance[2]/10)
        stance_binned_right.append(Humerusstance[0]/10)
        stance_binned_right.append(Humerusstance[1]/10)
        stance_binned_right.append(Humerusstance[2]/10)        
        stance_binned_right.append(Forearmstance[0]/10)
        stance_binned_right.append(Forearmstance[1]/10)
        stance_binned_right.append(Forearmstance[2]/10)        



        
        #left side:
        ##~ Thoraxstance = self.GenericAngleCalculation(self.GlobalOXYZ, self.OXYZThorax)      #elevatie, elevatievlak, axial
        Claviclestance = self.GenericAngleCalculation(self.OXYZThorax, self.Left_OXYZClavicle)
        Scapulastance = self.GenericAngleCalculation(self.Left_OXYZClavicle, self.Left_OXYZScapula)
        Humerusstance = self.GenericAngleCalculation(self.Left_OXYZScapula, self.Left_OXYZHumerus)
        Forearmstance = self.GenericAngleCalculation(self.Left_OXYZHumerus, self.Left_OXYZForearm)
        ##~ Forearmstance[1] = Forearmstance[1] - 90 # elevatievlak
        HumerusThorax = self.GenericAngleCalculation(self.OXYZThorax, self.Left_OXYZHumerus)
        stance_total_left = []
        
        #relative to parent
        stance_total_left[len(stance_total_left):] = Thoraxstance
        stance_total_left[len(stance_total_left):] = Claviclestance
        stance_total_left[len(stance_total_left):] = Scapulastance
        stance_total_left[len(stance_total_left):] = Humerusstance
        stance_total_left[len(stance_total_left):] = Forearmstance
        
        #relative to thorax
        stance_total_left[len(stance_total_left):] = HumerusThorax

        stance_binned_left = []
        stance_binned_left.append(Scapulastance[0]/10)
        stance_binned_left.append(Scapulastance[1]/10)
        stance_binned_left.append(Scapulastance[2]/10)
        stance_binned_left.append(Humerusstance[0]/10)
        stance_binned_left.append(Humerusstance[1]/10)
        stance_binned_left.append(Humerusstance[2]/10)        
        stance_binned_left.append(Forearmstance[0]/10)
        stance_binned_left.append(Forearmstance[1]/10)
        stance_binned_left.append(Forearmstance[2]/10)        

        blpositions = self.blPos[:]
        if stance_binned_right not in self.stanceCollectionBinnedRight:
            self.stanceCollectionBinnedRight.append(stance_binned_right)
            #~ print stance_total_right
            self.patternViewer.AddSpline(stance_total_right, blpositions)

        if stance_binned_left not in self.stanceCollectionBinnedLeft:
            self.stanceCollectionBinnedLeft.append(stance_binned_left)
            ##~ print stance_total_left
            ##~ print ""
            copiedblpositions = self.CopyBLLeftToRight(self.blPos)
            self.patternViewer.AddSpline(stance_total_left, copiedblpositions, updatewidget = False, referenceSpline = True)

        
    def GenericAngleCalculation(self, OXYZ1, OXYZ2, vis = False): #OXYZ1 = parent, OXYZ2=child
        #reference OXYZ1, measured angle OXYZ2
        point1 = OXYZ1[0]
        v1 = OXYZ1[2] #y-as
        v2 = OXYZ2[2]
        #~ print "v1: ", v1
        #~ print "v2: ", v2
        dottie = math.acos(vtk.vtkMath.Dot(v1,v2) / (math.sqrt(math.pow(v1[0],2) + math.pow(v1[1],2) + math.pow(v1[2],2))*\
                                                                            math.sqrt(math.pow(v2[0],2) + math.pow(v2[1],2) + math.pow(v2[2],2))))
        # we kunnen geen controle hoek invoeren omdat de overgang tussen bijv. abductie / anteflexie / adductie ill-defined is.
        # dus dan maar gewoon absoluut houden.
        dottie_axis = [0,0,0]
        vtk.vtkMath.Cross(v1, v2, dottie_axis)
        vtk.vtkMath().Normalize(dottie_axis)
        #~ controlehoek = -vtk.vtkMath.Dot(OXYZ1[3], dottie_axis) #determines (+) or (-) angle. OXYZ1 = Z-axis
        
        #~ elevatie = int(controlehoek * dottie*vtk.vtkMath.RadiansToDegrees())
        elevatie = vtk.vtkMath.DegreesFromRadians(dottie)
        point2 = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v2, 10))
        if vis and False:
            self.parent.playbackState.panel.annotation.\
                    VisualiseArc(point1, point2, self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v1, 10)), color = (0.0, 1.0, 0.0))
        #print "Elevation: ", elevatie
        #DO SIDE CHECK:
            #cross-product van v1 en v2 = Q
            #hoek tussen Q en humerus-X < 0 => elevatie = negatief, anders positief

        #~ v2_projected = self.ProjectedVectorOnPlane(OXYZ2, OXYZ1, 2, 2) #y-axis of child OXYZ2 on normal_y-plane of parent OXYZ 1
        v2_projected = dottie_axis
        v3 = OXYZ1[3]       #Z-axis
        point2 = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v2_projected, 20))
        dottie = math.acos(vtk.vtkMath.Dot(v3,v2_projected) / (math.sqrt(math.pow(v3[0],2) + math.pow(v3[1],2)+ math.pow(v3[2],2))*\
                                    math.sqrt(math.pow(v2_projected[0],2) + math.pow(v2_projected[1],2)+ math.pow(v2_projected[2],2))))
        dottie_axis = [0,0,0]
        vtk.vtkMath.Cross(v3, v2_projected, dottie_axis)
        vtk.vtkMath().Normalize(dottie_axis)
        controlehoek = round(vtk.vtkMath.Dot(v1, dottie_axis)) #determines + or - angle
        elevatievlak = (vtk.vtkMath.DegreesFromRadians(dottie) * controlehoek) + 90
        
        if vis and False:    
            point_temp = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v3, 20))
            # BLAUW
            self.parent.playbackState.panel.annotation.\
                    VisualiseLine(point1, point2, opacity = 1.0, color = (0.0, 0.0, 1.0))
            #ROOD:
            self.parent.playbackState.panel.annotation.\
                    VisualiseLine(point1, point_temp, opacity = 1.0, color = (1.0, 0.0, 0.0))
            #ROOD:
            self.parent.playbackState.panel.annotation.\
                    VisualiseArc(point1, point2, self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(OXYZ1[1], 20)), color = (1.0, 0.0, 0.0))
            #~ print elevatievlak
            #~ print controlehoek
        #print "Plane of elevation: ", elevatievlak
        
        #~ bepaal exorotatie:
        # corrigeer elevatie/elevatievlak
        t2_tmp = vtk.vtkTransform()
        t2_tmp.RotateWXYZ(elevatievlak, OXYZ1[2]) #elevatie-as roteren om de axiale as van parent (y)
        t2_tmp.Update()
        
        basicAxis = t2_tmp.TransformPoint(OXYZ1[1]) #elevatie-as X van parent
        #~ if vis:
            #~ print basicAxis
            #~ point3 = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(basicAxis, 10))
            #~ self.parent.playbackState.panel.annotation.\
                    #~ VisualiseArc(point1, point3, self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(OXYZ1[1], 10)), color = (0.0, 1.0, 0.0))
        
        transftmp = vtk.vtkTransform()        
        transftmp.RotateWXYZ(elevatie, basicAxis)
        transftmp.Update()
        
        v1_axial = OXYZ1[3]
        v2_axial = transftmp.TransformPoint(OXYZ2[3])
        
        #~ if vis:
            #~ point3 = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v1_axial, 10))
            #~ self.parent.playbackState.panel.annotation.\
                    #~ VisualiseArc(point1, point3, self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v2_axial, 10)), color = (0.0, 1.0, 1.0))
        dottie = math.acos(vtk.vtkMath.Dot(v1_axial,v2_axial) / (math.sqrt(math.pow(v1_axial[0],2) + math.pow(v1_axial[1],2) + math.pow(v1_axial[2],2))*\
                                                                            math.sqrt(math.pow(v2_axial[0],2) + math.pow(v2_axial[1],2) + math.pow(v2_axial[2],2))))
        dottie_axis = [0,0,0]
        vtk.vtkMath.Cross(v1_axial, v2_axial, dottie_axis)
        vtk.vtkMath().Normalize(dottie_axis)
        controlehoek = round(vtk.vtkMath.Dot(v1, dottie_axis))        
        axialerotatie = vtk.vtkMath.DegreesFromRadians(dottie) * controlehoek
        #~ if vis:
            #~ print "elevatie: ", elevatie
            #~ print "elevatievlak: ", elevatievlak
            #~ print "axiale rotatie: ", axialerotatie
        return [int(elevatie), int(elevatievlak), int(axialerotatie)]
        
        
        # in principe: z-axis projecteren op y-vlak en dan de hoek nemen tussen z-projected en z-parent
        # dichtbij 90 graden elevatie geeft dit Gimbal Lock. Te detecteren door de vectorlengte te meten
        # Deze is bij 0 graden elevatie 1, bij 90 graden is deze 0. Kleiner dan, zeg 0.25, dan moeten we op
        # zoek naar een veiligere maat
        # x-as projecteren op vlak y-parent. hoek tussen x-projected en x-parent.
        projection = self.ProjectedVectorOnPlane(OXYZ2, OXYZ1, 3, 2) #z-axis on normal_y plane : z-axis parent
        v4_projected = projection[0]        
        v4_length = projection[1]        
        if vis:
            print "v4_length: ", v4_length
        if v4_length < 0.3: #overstappen op x-as, ter vermijding van Gimbal Lock
            
            projection = self.ProjectedVectorOnPlane(OXYZ2, OXYZ1, 1, 2) #x-axis on normal_y plane : x-axis parent
            v4_projected = projection[0]            
            v4_length = projection[1]
            v5 = OXYZ1[1]
            if vis:
                print "New v4_length: ", v4_length

        else:
            v5 = OXYZ1[3] #z-axis
        vtk.vtkMath().Normalize(v4_projected)

        #~ point2 = self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(v4_projected, 10))
        #~ self.patternViewer.annotation2.\
                #~ VisualiseArc(point1, point2, self._root.baseCalc.AddPoints(point1, self._root.baseCalc.MultiplyVector(OXYZ1[3], 10)), color = (0.0, 0.0, 1.0))
        dottie = math.acos(vtk.vtkMath.Dot(v5,v4_projected) /\
                (math.sqrt(math.pow(v5[0],2) + math.pow(v5[1],2) + math.pow(v5[2],2))\
                *math.sqrt(math.pow(v4_projected[0],2) + math.pow(v4_projected[1],2) + math.pow(v4_projected[2],2))))
        #~ if (v4_projected[0] * v5[1] - v4_projected[1] * v5[0]) < 0:
            #~ dottie = -dottie
        
        #Side-check...
        dottie_axis = [0,0,0]
        vtk.vtkMath.Cross(v5, v4_projected, dottie_axis)
        vtk.vtkMath().Normalize(dottie_axis)    
        controlehoek = round(vtk.vtkMath.Dot(v1, dottie_axis)) #determines + or - angle
        #~ if abs(elevatie)>90:
            #~ controlehoek = -controlehoek
            
        axialerotatie = int(vtk.vtkMath.DegreesFromRadians(dottie) * controlehoek)
        if vis:
            print "Elevation: ", elevatie
            print "Axial rotation: ", axialerotatie
        
        return [elevatie, elevatievlak, axialerotatie]
    
    def CopyBLLeftToRight(self, blPos):
        for i in range(4, 14):            
            blPos[i] = blPos[i+10]
        blPos[24] = blPos[25]
        return blPos
        
            
    def SetupCoordinateSystems(self, blPos, side=2):
        #side = 0: Right
        #side = 1: Left
        #side = 2: both
        try:
            self.parent.playbackState.panel.annotation.ClearAll()
        except AttributeError:
            pass
                
        self.blPos = blPos
        self.DetermineMidPoints(blPos)
        self.DefineXYZThorax()
        self.blPos = self.FlipLeft(blPos)
        self.DefineXYZClavicle(side)
        self.DefineXYZScapula(side)
        self.DefineXYZHumerus(side)
        self.DefineXYZForearm(side)
        return self.blPos
        
        #~ print ""         
        #~ print self.OXYZThorax
        #~ print self.Right_OXYZClavicle
        #~ print self.Left_OXYZClavicle
        #~ print self.Right_OXYZScapula
        #~ print self.Left_OXYZScapula
        #~ print self.Right_OXYZHumerus
        #~ print self.Left_OXYZHumerus
        #~ print self.Right_OXYZForearm
        #~ print self.Left_OXYZForearm
        #~ return 1
        
    def VisualizeCoordinateSystems(self):
        #~ self.parent.playbackState.panel.annotation.\
                #~ VisualiseCoordinateSystem(self.OXYZThorax)
                
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Right_OXYZClavicle)
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Right_OXYZScapula)                
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Right_OXYZHumerus)                
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Right_OXYZForearm)          
                
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Left_OXYZForearm)
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Left_OXYZHumerus)
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Left_OXYZScapula)
        self.parent.playbackState.panel.annotation.\
                VisualiseCoordinateSystem(self.Left_OXYZClavicle)
           
        #~ pass
        
        
        

    def GetBLPos(self):
        blPos = []
        for landmark in self.landmarks:
            blPos.append(landmark['coords'])
        return blPos

    def DetermineMidPoints(self, blPos):
        #~ #run every frame
        #~ for landmark in self.landmarks:
            #~ landmark['coords'] = blPos[landmark['id']]
        self.midPXT8 = self._root.baseCalc.Average2Points(blPos[0], blPos[3])
        self.midIJC7 = self._root.baseCalc.Average2Points(blPos[1], blPos[2])
        self.Right_midELEM = self._root.baseCalc.Average2Points(blPos[10], blPos[11])
        


                
    def FlipLeft(self, blPos):        
        matrix1 = numpy.matrix([self.OXYZThorax[1], self.OXYZThorax[2], self.OXYZThorax[3]])
        matrix2 = numpy.transpose(matrix1)
        test = matrix1 * matrix2
        #~ print test
        BL_invertionlist = [14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 25]
        for i in BL_invertionlist:                    
            v = numpy.matrix(self._root.baseCalc.SubtractPoints(blPos[i], self.OXYZThorax[0]))
            v = v.transpose()
            #v = coords
            #~ self.parent.playbackState.panel.annotation.VisualisePoint((self._root.baseCalc.SubtractPoints(landmark['coords'], self.OXYZThorax[0])), "Start", (0,0,0))
            #v = numpy.invert(v)#v.transpose()
            #~ print "v: ", v
            v2 = matrix1 * v

            v2[2,0] = -v2[2,0]

            v2_flipped = matrix2 * v2
            #~ print "v2_flipped: ", v2_flipped
            v2_flipped = v2_flipped.transpose()
            v2_flipped = v2_flipped.tolist()[0]
            #~ self.parent.playbackState.panel.annotation.VisualisePoint([0,0,0], "origin", (0,0,0))
            #~ self.parent.playbackState.panel.annotation.VisualisePoint(v2_flipped, "eind", (0,0,0))
            coords_out = self._root.baseCalc.AddPoints(v2_flipped, self.OXYZThorax[0])
            blPos[i] = coords_out
            #~ self.parent.playbackState.panel.annotation.VisualisePoint(coords_out, landmark['s_name'], (0,0,0))
        self.Left_midELEM = self._root.baseCalc.Average2Points(blPos[20], blPos[21])
        return blPos
     
    def DefineXYZThorax(self):
        #Ot: The origin coincident with IJ
        coordThorax_O = self.blPos[1]
        #Yt: The line connecting the midpoint between PX and T8 
        # and the midpoint between IJ and C7, pointing upward.
        coordThorax_Y = self._root.baseCalc.SubtractPoints(self.midIJC7, self.midPXT8)
        coordThorax_Y = self._root.baseCalc.Normalize(coordThorax_Y)
        #Zt: The line perpendicular to the plane formed by
        #IJ, C7, and the midpoint between PX and T8, pointing to the Right.
        TempA = self._root.baseCalc.SubtractPoints(self.blPos[1], self.midPXT8)
        TempB = self._root.baseCalc.SubtractPoints(self.blPos[1], self.blPos[2])
        TempA = self._root.baseCalc.Normalize(TempA)        
        TempB = self._root.baseCalc.Normalize(TempB)        
        coordThorax_Z = [0,0,0]
        vtk.vtkMath.Cross(TempB, TempA, coordThorax_Z)
        coordThorax_Z = self._root.baseCalc.Normalize(coordThorax_Z)
        #Xt: The common line perpendicular to the Zt- and
        #Yt-axis, pointing forwards.
        coordThorax_X = [0,0,0]
        vtk.vtkMath.Cross(coordThorax_Y, coordThorax_Z, coordThorax_X)
        coordThorax_X = self._root.baseCalc.Normalize(coordThorax_X)
        #~ print "coordThorax_X: ", self._root.baseCalc.Length3DVector(coordThorax_X)
        #~ print "coordThorax_Y: ", self._root.baseCalc.Length3DVector(coordThorax_Y)
        #~ print "coordThorax_Z: ", self._root.baseCalc.Length3DVector(coordThorax_Z)        
        self.OXYZThorax = [coordThorax_O, coordThorax_X, coordThorax_Y, coordThorax_Z]
        



    def DefineXYZClavicle(self, side):
        if side == 0 or side == 2:
            #The origin coincident with SC
            coordClavicle_O = self.blPos[4]
            #Zc: The line connecting SC and AC, pointing to AC
            coordClavicle_Z = self._root.baseCalc.SubtractPoints(self.blPos[6], self.blPos[4])
            coordClavicle_Z = self._root.baseCalc.Normalize(coordClavicle_Z)
            #Xc: The line perpendicular to Zc and Yt, pointing forward
            #Note that the Xc-axis is defined with
            #respect to the vertical axis of the thorax (Ytaxis)
            #because only two bonylandmarks can be discerned at the clavicle.
            TempA = self._root.baseCalc.SubtractPoints(self.midIJC7, self.midPXT8)
            TempA = self._root.baseCalc.Normalize(TempA) #(Yt)       
            coordClavicle_X = [0,0,0]
            vtk.vtkMath.Cross(TempA, coordClavicle_Z, coordClavicle_X)
            coordClavicle_X = self._root.baseCalc.Normalize(coordClavicle_X)
            #Yc: The common line perpendicular to the Xc- and Zc-axis, pointing upward.
            coordClavicle_Y = [0,0,0]
            vtk.vtkMath.Cross(coordClavicle_Z, coordClavicle_X, coordClavicle_Y)
            coordClavicle_Y = self._root.baseCalc.Normalize(coordClavicle_Y)
            
            self.Right_OXYZClavicle = [coordClavicle_O, coordClavicle_X, coordClavicle_Y, coordClavicle_Z]
        
        if side == 1 or side == 2:
            #The origin coincident with SC
            coordClavicle_O = self.blPos[14]
            #Zc: The line connecting SC and AC, pointing to AC
            coordClavicle_Z = self._root.baseCalc.SubtractPoints(self.blPos[16], self.blPos[14])
            coordClavicle_Z = self._root.baseCalc.Normalize(coordClavicle_Z)
            #Xc: The line perpendicular to Zc and Yt, pointing forward
            #Note that the Xc-axis is defined with
            #respect to the vertical axis of the thorax (Ytaxis)
            #because only two bonylandmarks can be discerned at the clavicle.
            TempA = self._root.baseCalc.SubtractPoints(self.midIJC7, self.midPXT8)
            TempA = self._root.baseCalc.Normalize(TempA) #(Yt)       
            coordClavicle_X = [0,0,0]
            vtk.vtkMath.Cross(TempA, coordClavicle_Z, coordClavicle_X)
            coordClavicle_X = self._root.baseCalc.Normalize(coordClavicle_X)
            #Yc: The common line perpendicular to the Xc- and Zc-axis, pointing upward.
            coordClavicle_Y = [0,0,0]
            vtk.vtkMath.Cross(coordClavicle_Z, coordClavicle_X, coordClavicle_Y)
            coordClavicle_Y = self._root.baseCalc.Normalize(coordClavicle_Y)
            
            self.Left_OXYZClavicle = [coordClavicle_O, coordClavicle_X, coordClavicle_Y, coordClavicle_Z]
        
        
        
    def DefineXYZScapula(self, side):
        if side == 0 or side == 2:
            #The origin coincident with AA.
            coordScapula_O = self.blPos[7]
            #Zs: The line connecting TS and AA, pointing to AA.       
            coordScapula_Z = self._root.baseCalc.SubtractPoints(self.blPos[7], self.blPos[8])
            #coordScapula_Z = self._root.baseCalc.Normalize(coordScapula_Z)
            vtk.vtkMath.Normalize(coordScapula_Z)

            #Xs: The line perpendicular to the plane formed by AI, AA, and TS, pointing forward. 
            TempA = self._root.baseCalc.SubtractPoints(self.blPos[9], self.blPos[8])
            TempB = self._root.baseCalc.SubtractPoints(self.blPos[9], self.blPos[7])
            TempA = self._root.baseCalc.Normalize(TempA)        
            TempB = self._root.baseCalc.Normalize(TempB)        
            coordScapula_X = [0,0,0]
            vtk.vtkMath.Cross(TempA, TempB, coordScapula_X)
            coordScapula_X = self._root.baseCalc.Normalize(coordScapula_X)

            #Ys: The common line perpendicular to the Xs- and Zs-axis, pointing upward.
            coordScapula_Y = [0,0,0]
            vtk.vtkMath.Cross(coordScapula_Z, coordScapula_X, coordScapula_Y)
            coordScapula_Y = self._root.baseCalc.Normalize(coordScapula_Y)
            
            #~ #Faulty FOB coords...:
            #~ tmp = coordScapula_Z
            #~ coordScapula_Z = self._root.baseCalc.MultiplyVector(coordScapula_X, -1)
            #~ coordScapula_X = tmp
            
            self.Right_OXYZScapula = [coordScapula_O, coordScapula_X, coordScapula_Y, coordScapula_Z]
            
        if side == 1 or side == 2:
            #The origin coincident with AA.
            coordScapula_O = self.blPos[17]
            #Zs: The line connecting TS and AA, pointing to AA.       
            coordScapula_Z = self._root.baseCalc.SubtractPoints(self.blPos[17], self.blPos[18])
            #coordScapula_Z = self._root.baseCalc.Normalize(coordScapula_Z)
            vtk.vtkMath.Normalize(coordScapula_Z)

            #Xs: The line perpendicular to the plane formed by AI, AA, and TS, pointing forward. 
            TempA = self._root.baseCalc.SubtractPoints(self.blPos[19], self.blPos[18])
            TempB = self._root.baseCalc.SubtractPoints(self.blPos[19], self.blPos[17])
            TempA = self._root.baseCalc.Normalize(TempA)        
            TempB = self._root.baseCalc.Normalize(TempB)        
            coordScapula_X = [0,0,0]
            vtk.vtkMath.Cross(TempA, TempB, coordScapula_X)
            coordScapula_X = self._root.baseCalc.Normalize(coordScapula_X)

            #Ys: The common line perpendicular to the Xs- and Zs-axis, pointing upward.
            coordScapula_Y = [0,0,0]
            vtk.vtkMath.Cross(coordScapula_Z, coordScapula_X, coordScapula_Y)
            coordScapula_Y = self._root.baseCalc.Normalize(coordScapula_Y)
            
            #~ #Faulty FOB coords...:
            #~ tmp = coordScapula_Z
            #~ coordScapula_Z = self._root.baseCalc.MultiplyVector(coordScapula_X, -1)
            #~ coordScapula_X = tmp
            
            self.Left_OXYZScapula = [coordScapula_O, coordScapula_X, coordScapula_Y, coordScapula_Z]
            

    def DefineXYZHumerus(self, side):
        if side == 0 or side == 2:
            coordHumerus_O = self.blPos[24]
            #Yh1: The line connecting GH and the midpoint of EL and EM, pointing to GH.
            coordHumerus_Y = self._root.baseCalc.SubtractPoints(self.blPos[24], self.Right_midELEM)
            coordHumerus_Y = self._root.baseCalc.Normalize(coordHumerus_Y)

            #Xh1: The line perpendicular to the plane formed by EL, EM, and GH, pointing forward.
            coordHumerus_Ya = self._root.baseCalc.SubtractPoints(self.blPos[24], self.blPos[10])
            coordHumerus_Yb = self._root.baseCalc.SubtractPoints(self.blPos[24], self.blPos[11])
            coordHumerus_Ya = self._root.baseCalc.Normalize(coordHumerus_Ya)
            coordHumerus_Yb = self._root.baseCalc.Normalize(coordHumerus_Yb)
            coordHumerus_X = [0,0,0]
            vtk.vtkMath.Cross(coordHumerus_Yb, coordHumerus_Ya, coordHumerus_X)
            coordHumerus_X = self._root.baseCalc.Normalize(coordHumerus_X)

            #Zh1: The common line perpendicular to the Yh1- and Xh1-axis, pointing to the Right.
            coordHumerus_Z = [0,0,0]
            vtk.vtkMath.Cross(coordHumerus_X, coordHumerus_Y, coordHumerus_Z)
            coordHumerus_Z = self._root.baseCalc.Normalize(coordHumerus_Z)
            
            #Faulty FOB coords...:
            #~ tmp = coordHumerus_Z
            coordHumerus_Z = self._root.baseCalc.MultiplyVector(coordHumerus_Z, -1)
            coordHumerus_X = self._root.baseCalc.MultiplyVector(coordHumerus_X, -1)
            #~ coordHumerus_X = tmp

            self.Right_OXYZHumerus = [coordHumerus_O, coordHumerus_X, coordHumerus_Y, coordHumerus_Z]
            
        if side == 1 or side == 2:
            coordHumerus_O = self.blPos[25]
            #Yh1: The line connecting GH and the midpoint of EL and EM, pointing to GH.
            coordHumerus_Y = self._root.baseCalc.SubtractPoints(self.blPos[25], self.Left_midELEM)
            coordHumerus_Y = self._root.baseCalc.Normalize(coordHumerus_Y)

            #Xh1: The line perpendicular to the plane formed by EL, EM, and GH, pointing forward.
            coordHumerus_Ya = self._root.baseCalc.SubtractPoints(self.blPos[25], self.blPos[20])
            coordHumerus_Yb = self._root.baseCalc.SubtractPoints(self.blPos[25], self.blPos[21])
            coordHumerus_Ya = self._root.baseCalc.Normalize(coordHumerus_Ya)
            coordHumerus_Yb = self._root.baseCalc.Normalize(coordHumerus_Yb)
            coordHumerus_X = [0,0,0]
            vtk.vtkMath.Cross(coordHumerus_Yb, coordHumerus_Ya, coordHumerus_X)
            coordHumerus_X = self._root.baseCalc.Normalize(coordHumerus_X)

            #Zh1: The common line perpendicular to the Yh1- and Xh1-axis, pointing to the Left.
            coordHumerus_Z = [0,0,0]
            vtk.vtkMath.Cross(coordHumerus_X, coordHumerus_Y, coordHumerus_Z)
            coordHumerus_Z = self._root.baseCalc.Normalize(coordHumerus_Z)
            
            #Faulty FOB coords...:
            #~ tmp = coordHumerus_Z
            coordHumerus_Z = self._root.baseCalc.MultiplyVector(coordHumerus_Z, -1)
            coordHumerus_X = self._root.baseCalc.MultiplyVector(coordHumerus_X, -1)
            #~ coordHumerus_X = tmp

            self.Left_OXYZHumerus = [coordHumerus_O, coordHumerus_X, coordHumerus_Y, coordHumerus_Z]
            

    def DefineXYZForearm(self, side):
        if side == 0 or side == 2:
            coordForearm_O = self.blPos[13]
            #Yf : The line connecting US and the midpoint
            #between EL and EM, pointing proximally.
            coordForearm_Y = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.blPos[13])
            coordForearm_Y = self._root.baseCalc.Normalize(coordForearm_Y)

            #Xf : The line perpendicular to the plane through US,
            #RS, and the midpoint between EL and EM, pointing forward.
            TempA = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.blPos[13])
            TempB = self._root.baseCalc.SubtractPoints(self.Right_midELEM, self.blPos[12])   
            TempA = self._root.baseCalc.Normalize(TempA)
            TempB = self._root.baseCalc.Normalize(TempB)
            coordForearm_X = [0,0,0]
            vtk.vtkMath.Cross(TempB, TempA, coordForearm_X)
            coordForearm_X = self._root.baseCalc.Normalize(coordForearm_X)
            #*Hoe zorgen dat coordForearm_X naar voren wijst??
            
            #Zf : The common line perpendicular to the Xf and
            #Yf -axis, pointing to the Right.
            coordForearm_Z = [0,0,0]
            vtk.vtkMath.Cross(coordForearm_X, coordForearm_Y, coordForearm_Z)
            coordForearm_Z = self._root.baseCalc.Normalize(coordForearm_Z)

            self.Right_OXYZForearm = [coordForearm_O, coordForearm_X, coordForearm_Y, coordForearm_Z]
        
        if side == 1 or side == 2:
            coordForearm_O = self.blPos[23]
            #Yf : The line connecting US and the midpoint
            #between EL and EM, pointing proximally.
            coordForearm_Y = self._root.baseCalc.SubtractPoints(self.Left_midELEM, self.blPos[23])
            coordForearm_Y = self._root.baseCalc.Normalize(coordForearm_Y)

            #Xf : The line perpendicular to the plane through US,
            #RS, and the midpoint between EL and EM, pointing forward.
            TempA = self._root.baseCalc.SubtractPoints(self.Left_midELEM, self.blPos[23])
            TempB = self._root.baseCalc.SubtractPoints(self.Left_midELEM, self.blPos[22])   
            TempA = self._root.baseCalc.Normalize(TempA)        
            TempB = self._root.baseCalc.Normalize(TempB)        
            coordForearm_X = [0,0,0]
            vtk.vtkMath.Cross(TempB, TempA, coordForearm_X)
            coordForearm_X = self._root.baseCalc.Normalize(coordForearm_X)
            #*Hoe zorgen dat coordForearm_X naar voren wijst??
            
            #Zf : The common line perpendicular to the Xf and
            #Yf -axis, pointing to the Left.
            coordForearm_Z = [0,0,0]
            vtk.vtkMath.Cross(coordForearm_X, coordForearm_Y, coordForearm_Z)
            coordForearm_Z = self._root.baseCalc.Normalize(coordForearm_Z)

            self.Left_OXYZForearm = [coordForearm_O, coordForearm_X, coordForearm_Y, coordForearm_Z]
        
    def ProjectedVectorOnPlane(self, oxyzFirst, oxyzSecond, axis, planenormal):
        # Projecting an axis (1,2,3) of oxyzFirst on a plane with normal (1,2,3) of oxyzSecond.
        # Returns normalized vector on plane
        
        m1 = numpy.matrix([oxyzSecond[1], oxyzSecond[2], oxyzSecond[3]])
        m2 = m1.transpose()
        v1 = numpy.matrix([oxyzFirst[axis]])
        v1 = v1.transpose()
        tmp2 = m1 * v1
        tmp2[planenormal-1, 0] = 0.0        
        v2_projected = m2 * tmp2
        v2_projected = v2_projected.transpose()
        norm = lambda x: numpy.sqrt(numpy.square(v2_projected).sum())
        vectorlength = norm(v2_projected)
        v2_projected_list = v2_projected.tolist()[0]        
        # UNNORMALIZED!!! on purpose.
        return [v2_projected_list, vectorlength]
        
        #~ origin = self.oxyzSecond[0] #origin
        #~ point2 = self._root.baseCalc.AddPoints(origin, self._root.baseCalc.MultiplyVector(v2_projected, 10))
        #~ self.patternViewer.annotation2.\
                #~ VisualiseLine(point1, point2, opacity = 1.0, color = (0.0, 0.0, 0.0))
                
        #~ v3 = oxyzSecond[axis]
        #~ dottie = math.acos(vtk.vtkMath.Dot(v3,v2_projected) / (math.sqrt(math.pow(v3[0],2) + math.pow(v3[1],2) + math.pow(v3[2],2))*math.sqrt(math.pow(v2_projected[0],2) + math.pow(v2_projected[1],2) + math.pow(v2_projected[2],2))))
        #~ dottie_degrees = dottie*vtk.vtkMath.RadiansToDegrees()
        #~ return dottie_degrees
        #print "elevatievlak: ", dottie_degrees
        
    def MatchCoordinateSystemAtoB(self, oxyzFirst, oxyzSecond):
        #returns a transform that, when applied, transforms coordinate system xyzFirst
        #to xyzSecond, i.e. x=x, y=y, z=z.
        
        total_lmtransf = vtk.vtkLandmarkTransform()
        assen1 = vtk.vtkPoints()
        assen1.SetNumberOfPoints(4)
        assen1.SetPoint(0, oxyzFirst[0])
        assen1.SetPoint(1, oxyzFirst[1])
        assen1.SetPoint(2, oxyzFirst[2])
        assen1.SetPoint(3, oxyzFirst[3])
        
        assen2 = vtk.vtkPoints()
        assen2.SetNumberOfPoints(4)
        assen2.SetPoint(0, oxyzSecond[0])
        assen2.SetPoint(1, oxyzSecond[1])
        assen2.SetPoint(2, oxyzSecond[2])
        assen2.SetPoint(3, oxyzSecond[3])
        total_lmtransf.SetSourceLandmarks(assen1)
        total_lmtransf.SetTargetLandmarks(assen2)
        total_lmtransf.Update()
        
        total_matrix1 = total_lmtransf.GetMatrix()
        total_newtransf = vtk.vtkTransform()
        total_newtransf.SetMatrix(total_matrix1)
        return total_newtransf


    def ReadRootDirectory(self):
        if hasattr(sys, 'frozen') and sys.frozen:
            appdir, exe = os.path.split(sys.executable)
        else:
            dirname = os.path.dirname(sys.argv[0])
            if dirname and dirname != os.curdir:
                appdir = dirname
            else:
                appdir = os.getcwd()
        return appdir
        
    def ImportSplinesSmall(self):
        fname = os.path.join(self.appdir, 'splines_elbow.txt')
        importfile = open(fname, 'r', 1)        
        lines = importfile.readlines()
        for i in range (0, len (lines)/2):
            stance = eval(lines[i*2])
            blPos = eval(lines[i*2+1])
            self.patternViewer.AddSpline(stance, blPos, updatewidget = False, referenceSpline = False)
        importfile.close()
        self.patternViewer.gui.widgets.UpdateWidgets()
        self.patternViewer.gui.UpdateSequencePlot(9)
        #~ print self.patternViewer.Filters.jointAngleCollection[9]
        
        
    def ImportSplines1(self):
        fname = os.path.join(self.appdir, 'splines.txt')
        #~ fname = os.path.join(self.appdir, 'splines_elbow.txt')
        importfile = open(fname, 'r', 1)        
        lines = importfile.readlines()
        for i in range (0, len (lines)/2):
            stance = eval(lines[i*2])
            blPos = eval(lines[i*2+1])
            self.patternViewer.AddSpline(stance, blPos, updatewidget = False, referenceSpline = False)
        importfile.close()
        
        self.patternViewer.gui.widgets.UpdateWidgets()
        self.patternViewer.gui.UpdateSequencePlot(9)
        
    def ImportSplines2(self):
        fname = os.path.join(self.appdir, 'splines_passief.txt')
        #~ fname = os.path.join(self.appdir, 'splines_passief_short.txt')
        importfile = open(fname, 'r', 1)        
        lines = importfile.readlines()
        for i in range (0, len (lines)/2):
            stance = eval(lines[i*2])
            blPos = eval(lines[i*2+1])
            self.patternViewer.AddSpline(stance, blPos, updatewidget = False, referenceSpline = True)
        importfile.close()
        
        self.patternViewer.gui.widgets.UpdateWidgets()
        
    def ReadAngleFile(self, fname):
        importfile = open(fname, 'r', 1)
        lines = importfile.readlines()
        
        stancelist = []
        for i in range (0, len (lines)):
            stance = (lines[i]).split()
            stanceflt = []
            for j in stance:
                stanceflt.append(float(j))
            #elbow elevation plane not present in data
            stanceflt.insert(12, 0.0)
            
            #humerus-global angles not present in data
            stanceflt.append(0.0)
            stanceflt.append(0.0)
            stanceflt.append(0.0)
            
            #swap elevation and elevationplane
            for k in range(0, len(stanceflt)/3):
                stanceflt.insert(k*3, stanceflt[k*3+1])        
                stanceflt.pop(k*3+2)            
            
            #~ #clavicle angle may be flipped?
            #~ stanceflt[5] = -stanceflt[5]
            
            stancelist.append(stanceflt)
        return stancelist




    def AngleFiles(self):
        blPos_GenericSkeleton = [[112.0170752018814, 1.2942639374899696, 10.864873678096608],\
                                            [110.40771338957992, 1.9925669659434697, -4.6983268907973557],\
                                            [104.17419623891995, 3.7878726012110415, -15.40384202012592],\
                                            [94.999424758214758, 3.5293192897876215, 3.5045948263617586],\
                                            [110.53152652553526, 4.8646938632774566, -6.831116604254218],\
                                            [109.43318402993354, 17.037239972304867, -4.5305175872666599],\
                                            [109.78178754978204, 21.755615974354601, -6.7684137794569397],\
                                            [106.75205876719453, 23.105028309424345, -5.6459772829054975],\
                                            [99.148296487411216, 12.958210148467339, -7.5821107093384725],\
                                            [95.637864949303875, 13.839828031874202, 3.4522271706941279],\
                                            [102.5036233495958, 25.271488851532347, 26.27990416414816],\
                                            [102.46214664669861, 20.374542235794447, 26.939802533159135],\
                                            [111.03715508038803, 26.883934292485414, 52.351379968684689],\
                                            [107.75626381347377, 25.422313503800616, 54.181314916262998],\
                                            [109.75582826756268, 5.5777415653361402, -4.6864746758040381],\
                                            [108.81178197246609, 18.016754758656596, -4.2760757670688934],\
                                            [109.05551946108935, 21.612268201800749, -5.400613300882573],\
                                            [107.40872984648813, 23.284383912895052, -4.2049826313827872],\
                                            [99.248369642052154, 13.36978476428367, -5.3218347492054772],\
                                            [97.243158592270035, 15.239545826901447, 5.1814993097675419],\
                                            [110.38014942008698, 23.761123070506795, 29.731441484429581],\
                                            [107.61530111485308, 20.281164273344608, 29.651352763843853],\
                                            [121.47533522367939, 19.780253333764321, 51.361429936224702],\
                                            [120.18722060552903, 21.598985300192698, 52.46386136443202],\
                                            [109.47954544191421, 21.996081120041861, -2.32384462886629],\
                                            [106.83721641418667, -14.687957697267301, -2.8048070019125309]]
        self.patternViewer.ResetSkeleton(blPos_GenericSkeleton)

        fname = 'C:/Documents and Settings/Administrator/Mijn documenten/FobData/Frans2/test_ab.txt'
        stancelist = self.ReadAngleFile(fname)
        for stance in stancelist:
            blPos = self.patternViewer.GeneratePhysique(stance, showbones = False, returnBLpos = True)
            #~ self.patternViewer.AddSpline(stance, blPos, updatewidget = False, referenceSpline = False)
            self.SetupCoordinateSystems(blPos)
            self.Angles()
        
        #~ fname = 'C:/Documents and Settings/Administrator/Mijn documenten/FobData/Frans2/test_ab2.txt'
        #~ stancelist2 = self.ReadAngleFile(fname)
        #~ for stance in stancelist2:
            #~ blPos = self.patternViewer.GeneratePhysique(stance, showbones = False, returnBLpos = True)
            #~ ##~ self.patternViewer.AddSpline(stance, blPos, updatewidget = False, referenceSpline = True)
            #~ self.Angles(blPos)
        
        self.patternViewer.gui.widgets.UpdateWidgets()

        

class MyApp(wx.App):
    def OnInit(self):
        angles = AngleCalculations(None, None)
        angles.isMain = True
        angles.appdir = angles.ReadRootDirectory()
        angles.CreatePatternViewer()
        angles.baseCalc = BaseCalc.create(self, self)
        #~ angles.patternViewer.gui.OnLoadData1(None)
        
        ##~ angles.ImportSplinesSmall()   
        ##~ angles.ImportSplines2()
        ##~ angles.AngleFiles()
        
        return 1

if __name__ == "__main__":
    app = MyApp(0)
    app.MainLoop() 